#include "LinearHT.h"
#include <opencv/highgui.h>


LinearHT::LinearHT (int angleStep, int radiusStep, int directDetectSize)
		: _cosTable (angleStep), _sinTable (angleStep),
		_angleStep (angleStep), _radiusStep (radiusStep),
		_directDetectSize (directDetectSize),
		_houghArray (angleStep, vector< int > (radiusStep)),
		_tableIndex (directDetectSize * 8), _deltaAngle (CV_PI / _angleStep),
		_morphological (MorphologicalOperation::RECT, 5, 5)
{
	for (int i = 0; i < _angleStep; ++i) {
		double angle = _deltaAngle * i;
		_cosTable[i] = cos (angle);
		_sinTable[i] = sin (angle);
	}
	for (int delta = -directDetectSize, i = 0; delta < directDetectSize;
			++delta, ++i) {
		double r = sqrt (pow ((double)directDetectSize, 2) + pow ((double)delta, 2));
		double ang1 = acos (delta / r);
		double ang2 = (ang1 >= CV_PI / 2 ? ang1 - CV_PI / 2 : ang1 + CV_PI / 2);
		_tableIndex[i + 2 * directDetectSize] = _tableIndex[i] =
				ang2 / _deltaAngle;
		_tableIndex[i + 3 * directDetectSize] =
				_tableIndex[i + directDetectSize] = ang1 / _deltaAngle;
	}
	_houghArrayImage = cvCreateImage (cvSize (_angleStep * 2, _radiusStep), 8, 1);
	_localMaxImage = cvCreateImage (cvSize (_angleStep * 2, _radiusStep), 8, 1);

}

LinearHT::~LinearHT ()
{
	cvReleaseImage (&_houghArrayImage);
	cvReleaseImage (&_localMaxImage);
}

vector< CvPoint2D64f > LinearHT::findLine (IplImage * inImage)
{
	fillHoughArray (inImage);
	return getLines ();
}

void LinearHT::fillHoughArray (IplImage * inImage)
{
	_deltaRad = sqrt (pow ((double)inImage->width, 2) +
			pow ((double)inImage->height, 2)) / _radiusStep;
	_maxHoughValue = 0;
	double centerX = inImage->width / 2;
	double centerY = inImage->height / 2;

	// Iterate the pixels and calculate hough of pixels value 255
	for (int u = 0; u < inImage->width; u++)
		for (int v = 0; v < inImage->height; v++)
			if (CV_IMAGE_ELEM (inImage, uchar, v, u) == 255) {
				double x = (double)u - centerX;
				double y = centerY - (double)v;
				for (int delta = -_directDetectSize, i = 0;
						delta < _directDetectSize; ++delta, ++i) {
					if (CV_IMAGE_ELEM (inImage, uchar,
							v - _directDetectSize, u + delta) == 255)
						_fillHoughArray (x, y, i);
					if (CV_IMAGE_ELEM (inImage, uchar,
							v + _directDetectSize, u - delta) == 255)
						_fillHoughArray (x, y, i + 2 * _directDetectSize);
					if (CV_IMAGE_ELEM (inImage, uchar, v + delta,
							u + _directDetectSize) == 255)
						_fillHoughArray (x, y, i + _directDetectSize);
					if (CV_IMAGE_ELEM (inImage, uchar, v - delta,
							u - _directDetectSize) == 255)
						_fillHoughArray (x, y, i + 3 * _directDetectSize);
				}
			}
}

vector< CvPoint2D64f > LinearHT::getLines ()
{
	double rate = 255.0 / _maxHoughValue;
	for (int u = 0; u < _angleStep; u++)
		for (int v = 0; v < _radiusStep; v++) {
			CV_IMAGE_ELEM (_houghArrayImage, uchar, v, u) = 
					(int)(rate * _houghArray[u][v]);
			CV_IMAGE_ELEM (_houghArrayImage, uchar, _radiusStep - 1 - v, u + _angleStep) = 
					(int)(rate * _houghArray[u][v]);
		}
	IplImage* _houghImageRGB = cvCreateImage (cvGetSize (_houghArrayImage), 8, 3);
	cvCvtColor (_houghArrayImage, _houghImageRGB, CV_GRAY2RGB);

	_thresh.process (_houghArrayImage, _houghArrayImage);
	_morphological.close (_houghArrayImage, _houghArrayImage);
	cvShowImage ("Hough Threshed", _houghArrayImage);


	CvMemStorage* storage = cvCreateMemStorage(0);
	CvSeq* contour = 0;
	cvFindContours (_houghArrayImage, storage, &contour, sizeof(CvContour));
	vector< CvPoint2D64f > lineArguments;
	for (; contour != 0; contour = contour->h_next) {
		CvMoments mom;
		cvMoments (contour, &mom, 1);
		double ang = mom.m10 / mom.m00 * _deltaAngle;
		if (ang > CV_PI / 4 && ang < CV_PI * 5 / 4) {
			double rad = mom.m01 / mom.m00 * _deltaRad;
			rad -= _radiusStep * _deltaRad / 2;
			if (ang >= CV_PI) {
				ang -= CV_PI;
				rad = - rad;
			}
			lineArguments.push_back (cvPoint2D64f (ang, rad));
			CV_IMAGE_ELEM (_houghImageRGB, uchar, (int)((rad + _radiusStep * _deltaRad / 2) / _deltaRad), (int)(ang / _deltaAngle) * 3) = 255;
			CV_IMAGE_ELEM (_houghImageRGB, uchar, (int)((rad + _radiusStep * _deltaRad / 2) / _deltaRad), (int)(ang / _deltaAngle) * 3 + 1) = 0;
			CV_IMAGE_ELEM (_houghImageRGB, uchar, (int)((rad + _radiusStep * _deltaRad / 2) / _deltaRad), (int)(ang / _deltaAngle) * 3 + 2) = 255;
		}
	}
// 	cvDrawLine (_houghImageRGB, cvPoint (0, _localMaxImage->height / 2),
// 	 		cvPoint (_localMaxImage->width, _localMaxImage->height / 2), cvScalar (0, 0, 150));
	cvDrawLine (_houghImageRGB, cvPoint (_houghImageRGB->width / 2, 0),
	  		cvPoint (_houghImageRGB->width / 2, _houghImageRGB->height), cvScalar (0, 0, 150));
// 	cvDrawLine (_houghImageRGB, cvPoint (_houghImageRGB->width / 4, 0),
// 			cvPoint (_houghImageRGB->width / 4, _houghImageRGB->height), cvScalar (0, 0, 150));
	cvShowImage ("Hough RGB", _houghImageRGB);
	cvReleaseImage (&_houghImageRGB);
	return lineArguments;

// 	cvCopy (_houghArrayImage, _localMaxImage);
// 	for (int v = 1; v < _houghArrayImage->height; ++v) {
// 		for (int u = 0; u < _houghArrayImage->width; ++u) {
// 			for (int i = -3; i < 3; ++i) {
// 				_compareAndClear (u, v, u + i, v - 3);
// 				_compareAndClear (u, v, u + i, v + 3);
// 				_compareAndClear (u, v, u - 3, v + i);
// 				_compareAndClear (u, v, u + 3, v + i);
// 			}
// // 			for (int i = -2; i < 2; ++i) {
// // 				_compareAndClear (u, v, u + i, v - 2);
// // 				_compareAndClear (u, v, u + i, v + 2);
// // 				_compareAndClear (u, v, u - 2, v + i);
// // 				_compareAndClear (u, v, u + 2, v + i);
// // 			}
// 		}
// 	}
// 
// 	cvShowImage ("Local Max", _localMaxImage);
// 	_thresh.process (_localMaxImage, _localMaxImage);
// 
// 	_morphological.dilate (_localMaxImage, _localMaxImage);
// 	cvShowImage ("Local Max Threshed", _localMaxImage);

}

vector< vector< int > >& LinearHT::getHoughArray ()
{
	return _houghArray;
}

void LinearHT::_fillHoughArray (double x, double y, int neighIndex)
{
	int iStart = _tableIndex[(neighIndex + 1) % _tableIndex.size ()];
	int iCenter = _tableIndex[neighIndex % _tableIndex.size ()];
	int iEnd = _tableIndex[(neighIndex - 1) % _tableIndex.size ()];
	int scSteps = (iCenter + _angleStep - iStart) % _angleStep;
	int ceSteps = (iEnd + _angleStep - iCenter) % _angleStep;
// 	iStart = iEnd = iCenter;
	int i = iStart;
	bool passCenter = false;
	do {
		double r = _cosTable[i] * x + _sinTable[i] * y;
		int j = (int) floor (r / _deltaRad) + _radiusStep / 2;
		int weight = 10;
// 		if (!passCenter)
// 			weight = 10.0 - (double)((iCenter + _angleStep - i) % _angleStep) / scSteps * 10;
// 		else
// 			weight = 10.0 - (double)((i + _angleStep - iCenter) % _angleStep) / scSteps * 10;
		if (weight > 0) {
			_houghArray[i][j] += weight;
			if (j > 0)
				_houghArray[i][j - 1] += weight;
			if (j < _radiusStep - 1)
				_houghArray[i][j + 1] += weight;

			if (_houghArray[i][j] > _maxHoughValue)
				_maxHoughValue = _houghArray[i][j];
			if (_houghArray[i][j - 1] > _maxHoughValue)
				_maxHoughValue = _houghArray[i][j - 1];
			if (_houghArray[i][j + 1] > _maxHoughValue)
				_maxHoughValue = _houghArray[i][j + 1];
		}
		if (i == iCenter) passCenter = true;
		i = (i + 1) % _angleStep;
	} while (i != iEnd);
}

void LinearHT::_compareAndClear (int x, int y, int nx, int ny)
{
	if (ny < 0 || ny >= _houghArrayImage->height)
		return;
	if (nx < 0 || nx >= _houghArrayImage->width)
		return;
	if (CV_IMAGE_ELEM (_houghArrayImage, uchar, y, x) > CV_IMAGE_ELEM (_houghArrayImage, uchar, ny, nx) - 3)
		CV_IMAGE_ELEM (_localMaxImage, uchar, ny, nx) = 0;
}